A  Variational  Approach  to  Path  Planning  in 
Three  Dimensions  Using  Level  Set  Methods 


Thomas  Cecil* 

ICES,  UT  Austin.  Austin,  TX  78712, 
and 

Daniel  E.  MarthaleL 

ACS-UMS,  Northrop  Grumman  Corp,  Rancho  Bernardo,  CA 

December  8,  2004 


Abstract 

In  this  paper  we  extend  the  two  dimensional  methods  set  forth  in  [4], 
proposing  a  variational  approach  to  a  path  planning  problem  in  three 
dimensions  using  a  level  set  framework.  After  defining  an  energy  integral 
over  the  path,  we  use  gradient  flow  on  the  defined  energy  and  evolve  the 
entire  path  until  a  locally  optimal  steady  state  is  reached.  We  follow 
the  framework  for  motion  of  curves  in  three  dimensions  set  forth  in  [2], 
modified  appropriately  to  take  into  account  that  we  allow  for  paths  with 
positive,  varying  widths.  Applications  of  this  method  extend  to  robotic 
motion  and  visibility  problems,  for  example.  Numerical  methods  and 
algorithms  are  given,  and  examples  are  presented. 


1  Introduction 

This  paper  extends  results  seen  in  [4]  to  path  planning  in  three  dimensions.  Path 
planning  in  an  obstacle-ridden  environment  while  simultaneously  attempting  to 
search  is  an  inherently  difficult,  and  well- studied  problem.  In  particular,  in  the 
field  of  unmanned  aerial  vehicle  path  planning,  many  different  solution  tech¬ 
niques  have  been  studied.  Potential  field  path  planning  methods  have  appeared 
frequently  in  the  literature  [22],  but  are  plagued  with  inherent  limitations  [17]. 
Probabilistic  road  mapping  [15]  is  a  technique  which  uses  a  heuristic  method  to 
generate  a  road  map  through  the  space,  then  searches  to  find  the  lowest  cost 
path.  The  heuristic  nature  of  the  path  generation  leads  to  a  difficulty  in  char¬ 
acterizing  the  algorithms  in  terms  of  performance,  robustness,  complexity  and 
reliability  [14].  An  optimization-based  technique  using  a  mixed  integer  linear 
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programming  (MILP)  method  has  recently  been  shown  to  perform  quite  well  in 
specific  instances  [26], [10].  This  method  combines  linear  programming  with  the 
ability  of  constraining  some  subset  of  the  state  variables  to  be  integers. 

In  general,  path  planning  methods  tend  to  fall  in  a  spectrum  with  complex 
direct  optimization  approaches  on  one  side  and  more  pragmatic  heuristic  ap¬ 
proaches  on  the  other.  Most  approaches  fall  somewhere  in  between  the  two 
extremes  and  make  use  of  both  heuristic  and  optimization  based  components. 
Combining  these  components  can  lead  to  some  guarantees  on  performance  and 
robustness  with  reduced  complexity  and  computation  time.  Also,  and  perhaps 
most  relevant,  is  the  fact  that  rarely,  if  ever  is  the  globally  optimal  path  ever 
required.  In  practice,  most  applications  require  a  process  that  produces  a  rea¬ 
sonable  result  with  the  cost  of  the  solution  increasing  with  longer  solve  time. 

The  general  problem  of  finding  the  optimal  path  through  a  domain  under 
some  given  constraints  has  many  applications.  Given  that  the  domain  is  not 
homogeneous,  i.e.  there  is  an  associated  cost  function  to  the  path  in  the  domain, 
the  general  solution  begins  to  increase  rapidly  in  complexity. 

A  specific  instance  of  the  general  problem  is  that  of  finding  an  optimal- 
path  map  for  a  known  environment.  The  optimal-path  map  for  a  known  three- 
dimensional  terrain  is  a  function  oo(x,y,z)  whose  values  describe  how  to  best 
reach  a  goal  point  from  the  location  (x,y,z).  Optimality  in  this  case  could  be 
shortest  path,  least  visibility  from  above,  largest  patrol  area,  etc.  Previous 
work  on  true  optimal-path  maps  for  autonomous  robotics  have  almost  always 
investigated  restricted  cases. 

Another  field  of  relevance  is  that  of  robotic  motion.  In  [16]  path  planning 
for  robots  was  studied  using  level  sets  where  there  were  objects  to  be  avoided 
in  the  domain.  The  method  of  solution  was  to  construct  a  weighted  distance 
function  over  the  entire  domain  and  then,  from  a  final  position,  back  propagate 
the  solution  perpendicular  to  the  level  sets  of  the  distance  function,  resulting  in 
an  optimally  shortest  path.  Path  planning  algorithms  for  mobile  robots  are  also 
described  in  [19], [3], [18].  Also,  in  the  context  of  manipulators  there  has  been 
path  planning  research  done  within  a  variational  framework  [28] . 

In  [31]  the  framework  for  studying  visibility  and  its  dynamics  using  level  sets 
was  established.  In  [6]  various  variational  problems  were  approached  using  the 
framework  established  in  [31].  In  [6]  a  parameterized  path  planning  algorithm 
was  introduced  that  treats  the  path  as  a  finite  union  of  multiple  observers  which 
are  evolved  so  as  to  maximize  the  accumulated  visibility  along  the  path.  See 
also  [33]  for  a  path  planning  algorithm  based  on  visibility. 

In  this  paper,  we  investigate  the  general  problem  of  finding  a  “search  path” 
through  a  domain  where  we  know  some  information  about  where  targets  and 
obstacles  may  be  located.  An  agent  searching  such  a  domain  would  want  to 
have  a  path  that  satisfies  being  shortest  with  having  a  high  confidence  of  finding 
targets  while  avoiding  obstacles.  The  searcher  can  only  “see”  a  finite  distance 
about  it  at  any  given  point,  and  this  distance  may  vary  spatially  according 
to  local  weather  conditions,  altitude,  etc.  Therefore,  we  wish  to  generate  an 
optimal  path  that  gives  a  certain  level  of  confidence  of  locating  targets,  while 
simultaneously  avoiding  obstacles,  which  will  be  determined  via  the  information 
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we  know  about  the  domain. 

Computationally  we  represent  the  path,  T,  as  in  intersection  of  the  sets 
{x\ (j)(x)  =  0}  fl  {x\/ip(x)  =  0}  of  two  level  set  functions  0,  ip  :  M3  — ►  M.  This 
framework  was  originally  established  in  [2]  and  [7].  However,  the  extension  of 
their  method  to  our  case  requires  that  we  evolve  a  tube,  not  just  a  codimension- 
2  curve,  whose  axis  is  T  and  whose  boundary  represents  the  edge  of  the  “visible” 
region  of  the  observer  located  on  T. 

The  remainder  of  the  paper  is  organized  as  follows,  in  the  next  section, 
we  formulate  the  search  path  problem  in  a  general  framework,  with  general 
metrics  describing  the  optimization,  and  constraints  stemming  from  the  search 
path  problem.  Following  that,  we  introduce  the  level  set  method,  and  then  our 
algorithm.  We  present  simulations  of  canonical  examples  demonstrating  the 
method  and  conclude  with  some  remarks  about  the  generality  of  the  method. 


2  Problem  Formulation 


The  general  path  planning  problem  has  had  many  formulations.  For  a  given  set 
H  G  M3,  we  seek  a  path  Y  :  [0, 1]  — »  M3,  with  the  following  properties: 

1.  Optimize  some  function  of  Y  (Arclength,  Curvature,  etc.). 

2.  Given  an  a  priori  distribution,  P,  on  £2,  maximize 


P(pc)dx 


where  Sr  =  {x  £  £2  :  \x  —  T|  <  c(x)}  ,  where  c(x)  is  the  radius  of  the  set 
“cut-out”  of  the  domain  by  the  path  Y. 

We  note  that  our  work  on  this  problem  was  motivated  by  the  task  of  com¬ 
puting  optimal  search  strategies  in  the  presence  of  a  priori  knowledge  [20] .  The 
function  P  represents  any  knowledge  of  the  search  domain,  Q.  Possible  choices 
for  the  optimization  would  include  minimal  arclength  and  minimal  curvature. 
Also,  we  note  that  this  encompasses  obstacle  avoidance  when  2  is  minimized  or 
the  sign  of  P  is  changed. 


2.1  Level  Set  Formulation 


The  search  path  Y  will  be  represented  by  the  intersection  of  the  0  level  sets  of 
two  functions  0,  ip  :  Q  — »  M.  This  follows  the  framework  established  in  [2]  and 
[7].  Given  initial  functions  <fi(t  =  O),^^  =  0),  and  an  energy  F(0, ip)  to  be 
minimized /maximized,  we  use  the  method  of  gradient  descent /ascent  to  arrive 
at  a  set  of  coupled  PDEs  of  the  form 


d(j)  dE  d'tp  dE 

dt  dcj)  ’  dt  +  dip  5 


(1) 
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where  ^  are  taken  from  the  Euler-Lagrange  equations.  These  PDEs  are 
then  evolved  to  steady  state  resulting  in  0,  i);  obtaining  local  minima.  Most  of 
our  variational  problems  will  be  nonconvex,  so  the  initial  choices  of  </>,  ^  will 
determine  the  local  minima  in  which  we  finish.  The  numerical  methods  for 
solving  (1)  will  be  discussed  later. 

2.2  Examples  of  Energies  and  PDEs 

The  energy  representing 


/, 


P{x)dx 


(2) 


will  always  be  included  in  our  variational  formulation.  First,  we  assume  0,  ^ 
are  weighted  signed  distance  functions,  and  that  they  are  perpendicular,  i.e. 
V0  •  =  0.  Given  this  assumption,  we  can  see  that 


X(Sv)  =  H(r-\\{ct>,m, 


(3) 


where  x  is  th e  characteristic  function,  and  H  is  the  Heaviside  function,  and  in 
practice  we  use  ||</>,  ^||  =  y '(j)2  +  Here,  the  width  of  the  tube  r  is  a  constant. 
We  note  that  this  results  in  a  cylindrical  path  whose  cross  sections  are  circles 
of  radius  r,  whose  center  axis  is  T.  See  Figure  1  for  a  sample  tube  construction. 
If  other  norms  were  used  instead  of  1 2  then  we  would  have  tubes  whose  cross 
sections  would  be  other  objects,  such  as  squares  when  using  the  l\  norm. 

Our  integral  (2)  can  then  be  written  as 


f  P(x)dx  =  [  H(r-  v^2  +  V’2)  P(x)  dx, 

Jsr 


(4) 


where  we  have  integrated  over  the  entire  domain  Q.  To  maximize  this  integral 
we  perform  gradient  ascent  and  arrive  at  the  PDEs 


fa  =  P(x)5(r  -  Jfi2  +t/>2)  j==  (5a) 

V fi  + 

fit  =  P(x)S(r  -  y/fi2  +  t/i2)  j=  =,  (5b) 

V  fiz  +  fi 

where  S(y)  =  H'(y)  denotes  the  Dirac  delta  function.  Intuitively,  solving  (5) 
attempts  to  move  the  set  {x\ yV2  +  V;2}  =  r  away  from  the  0  level  set  where 
P  >  0,  so  that  fs  P  becomes  larger  as  time  progresses. 

Note  that  we  have  made  the  assumptions  that  0,-0  are  weighted  signed 
distance  functions,  and  that  V0  •  =  0.  It  is  necessary  for  these  assumptions 

to  hold  for  (4)  to  be  valid.  Therefore,  we  need  to  enforce  these  conditions  during 
the  PDE  evolutions.  The  way  we  do  this  is  by  periodically  solving  a  set  of  PDEs 
whose  steady  state  solutions  satisfy  the  necessary  criteria. 
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Figure  1:  The  graphs  of  {(/)  =  0}  (red  xz  plane),  {'ip  =  0}  (green  sinusoidal 
plane),  and  {||(</>,  7/OII2  =  0.2}  (blue  tube). 


When  we  say  that  0  is  a  signed  distance  function,  we  mean  that  (p  satisfies 


1 

R(x)’ 


(6) 


where  R(x )  >  0,  with  boundary  condition  given  by  { <p(x )  =  0|x  E  T},  and  that 
sign(cp)  is  specified  in  Q.  One  way  of  solving  this  is  to  solve  the  PDE 


<k  +  (Vw>V0 1  -  =  0,  (7) 

to  steady  state,  where  S(x)  is  a  regularized  signum  function,  <p(x,t  =  0)  =  </>o 
in  £2,  and  (p(x  E  T)  =  0.  The  solution,  (p^,  is  a  signed  distance  function  with 
distance  measured  on  the  sets  {ip  =  c},Vc  E  M.  Similarly,  to  enforce  that  the 
level  sets  of  p  are  perpendicular  to  the  0  level  set  of  p  we  can  solve  the  following 
PDE  to  steady  state 


4>t  +  s(jP) 


IW>| 


•  V0  =  0, 


(8) 


with  cp(x,t  =  0)  =  0o  in  ^  and  p(t  >  0)|^,=o  =  (p(t  =  0)|^,=o,  thus  yielding 


IW>| 


•  V0  =  0, 


We  will  discuss  numerical  solvers  for  these  problems  later. 


(9) 
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Another  common  energy  term  to  be  minimized  is  the  length  of  T.  We  can 
write 


|F|  =  [  ^)^)|Pv^V0||V^|  dx,  (10) 

Jn 


where 


PV=I- 


v  <S>  v 


\V[ 


(11) 


is  the  orthogonal  projection  matrix  projecting  onto  the  plane  with  normal  v.  In 
standard  codimension- 1  level  set  methods  the  mean  curvature  motion 


ut  =  \\7u\k  (12) 

can  be  derived  by  minimizing  an  integral  analogous  to  (10)  by  gradient  descent, 
resulting  in 


ut  =  5(u)k, 


(13) 


and  then  replacing  the  S  function  by  \\7u\  (here  k  is  the  signed  mean  curvature 
of  the  codimension- 1  surface).  In  our  framework  it  was  shown  in  [2]  that  when 
minimizing  (10)  one  ends  up  with  a  diagonal  matrix  of  5  functions  that  can  be 
replaced  by  a  matrix  serving  the  analogous  role  of  \Vu\  in  the  codimension- 1 
case,  which  will  result  in  the  PDEs 

4>t  +  kN  •  V0  =  0  (14a) 

'ipt  +  kN  •  =  0,  (14b) 


where  kN  is  curvature  times  the  normal  vector  of  T,  i.e.  after  applying  gradient 
descent  we  arrive  at 


(  4>t  \  (  s((p)S(ip)  o  )  (  v'  (iCvllv^l)  ) 

UJ  l  o  )  (  -v-  (jg^iv^i)  j ' 


Then  the  matrix  of  S  functions  is  replaced  by 


IV0I 

l^vWbl 

v<yvp 

|Pv*V^||V0| 


Vc/>-vv> 

|Pv^V0||V^| 

|Vbl 

\Pv^<f>\ 


which  is  a  symmetric  positive  definite  matrix,  indicating  that  we  are  still  fol¬ 
lowing  a  gradient  descent  direction  minimizing  (10). 

We  note  that  the  vector  kN  can  be  found  by  taking  the  tangent  vector 


V'i/j  x  V(f> 

iw  x  v^r 
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and  deriving 


VTi  -T 
VT2  •  T 
VTs-T 


irji 

kN  =  ——  =  VT  •  T  = 
as 


(15) 


where  T{  is  the  ith  component  of  T  and  8  is  an  parameterization  of  T,  see  [1] ,  [2] 
for  more  details. 

For  certain  problems  one  may  want  to  control  the  magnitude  of  k  along  the 
path.  Energies  to  be  minimized  in  this  case  could  be  of  the  form 

[  ${4>)g(k)  dx,  (16) 

Jn 

where  g  is  a  non-negative  function  of  k  such  as  \k\p,p  >  0.  The  PDEs  resulting 
from  (16)  are  fourth  order  involving  second  partial  derivatives  of  k. 

In  general  we  will  evolve  (5)  with  its  right  hand  side  augmented  by  adding 
weighted  terms  that  are  found  from  the  additional  energy  minimizations/maximizations 
that  each  particular  problem  demands. 


3  Numerical  Methods 

The  PDEs  found  in  section  2.2  are  generally  Hamilton- Jacobi  equations.  To 
discretize  them  we  construct  a  uniform  rectangular  grid  on  D.  Viscosity  solu¬ 
tions  for  these  types  of  equations  have  been  studied  well  [9],  [12],  and  numerical 
methods  that  converge  to  the  viscosity  solution  have  been  implemented  [8],  [27], 
[24].  We  use  these  methods  to  solve  our  equations.  In  general  they  consist  of 
upwind  type  spatial  discretizations  and  explicit  Runge-Kutta  time  discretiza¬ 
tions,  and  also  fast  sweeping  solutions  to  find  steady  state  solutions.  We  note 
that  the  notation  below  will  refer  to  (p  as  the  unknown  function  where  ip  is  fixed, 
but  all  algorithms  and  PDEs  are  also  applied  to  ip  with  <p  fixed. 

3.1  Advancement  of  Time  Dependent  PDEs  Resulting  from 
Gradient  Flows 

The  level  set  problem  formulations  found  in  [2], [7]  involve  only  one  level  set 
of  interest,  the  set  To  =  {( p(x )  =  0}  D  {' xp(x )  =  0}.  Thus  the  PDEs  to  be 
evolved  only  involve  S  functions  that  have  support  localized  near  To.  However, 
for  our  problem,  (5)  includes  a  S  functions  whose  support  lies  near  the  set 
Sr  =  {x\  ||(0,  V;)|| 2  =  r},  when  we  use  a  numerical  approximation  to  S.  When 
(5)  is  combined  with  a  Lagrange  multiplier  term  derived  from  minimizing  A|To|, 
then  we  have  a  PDE  system  with  5  functions  localized  near  To  also. 

The  goal  is  simultaneously  evolve  all  the  PDEs  derived  from  gradient  flow 
on  the  energy  integrals,  as  well  as  from  the  distance  function  and  perpendic¬ 
ularity  requirements.  We  have  chosen  to  use  a  splitting  technique  similar  to 
that  in  [4].  The  general  idea  introduced  for  a  2d  domain  there  was  to  evolve 
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Figure  2:  Tube  Sr  around  To  (dotted  line),  with  three  curves, 

r(ai)a2),r(/3l)/32),r(7l)72),  that  lie  on  Sr,  i.e.  \\(ai,a2)\\2  =  ||(/?i, AOIk  = 
||(7i,72)||2  =  r. 


near  the  — r,  0,  r  level  sets  separately,  while  enforcing  (or  pseudo-enforcing) 
the  distance  function  requirement  in  between  advancements.  In  3d  we  have 
not  only  the  added  burden  of  enforcing  perpendicularity,  but  also  an  infinite 
number  of  codimension-2  curves,  =  {4>(x)  =  a}  D  {^(x)  =  6},  where 

||  (a,  fe)  || 2  =  r,  of  which  the  tube  Sr  is  composed.  Our  approach  numerically  will 
be  to  approximate  these  infinite  number  of  curves  by  a  finite  number.  Also,  the 
perpendicularity  requirement  will  be  met  by  also  solving  (9),  along  with  (6),  in 
between  advancements  near  the  curves  T 0  or  T^).  See  Figure  2. 

Thus  if  we  are  using  the  1 2  norm  to  determine  Sr,  then  we  discretize  a 
circular  cross  section  of  Sr  (found  by  taking  T(y)  •  (x  —  y)  for  a  point  y  G  To) 
into  M  points  by 

{(a*,  ^i)}iL  1  {cos(0q  +  2iri/M),  sm(0o  +  (17) 

where  #0  can  be  chosen  arbitrarily.  Then  we  split  the  evolution  of  Tr  into  M 
separate  evolutions  near  the  curves  {c f>  =  a^}  D  {ip  =  bi}.  After  0  has  been 
advanced  for  one  timestep  using  an  explicit  Runge-Kutta  method  near 
we  perform  pseudo-reinitialization  (as  was  done  in  the  2d  case).  This  pseudo- 
reinitialization  propagates  the  information  from  to  the  rest  of  12.  We 

repeat  the  advancement  and  pseudo-reinitialization  for  i/j  near  as  well 

before  advancing  near  another  T(a  &  )  for  j  ^  i.  In  practice  we  try  to  choose 
the  (aj,  bj )  so  that  (a*,  bi)  •  (aj,  bj)  is  minimized. 

In  order  to  avoid  the  discretization  of  the  5  function  we  modify  the  PDE 
(5a),  for  example,  by  replacing  the  S(r  —  ||(</>,  VOID  with  |V0| Cr(r  —  ||(0, '0)11), 
where  Cr(x)  is  a  cutoff  function,  as  described  in  [25]  with  support  over  points 
where  \x\  <  r.  This  allows  all  level  sets  of  0  to  move  with  the  same  speed,  and 
still  maintains  the  local  nature  (near  r^.^))  of  the  evolution.  The  term  |V0| 
is  discretized  using  a  Godunov  numerical  Hamiltonian. 

Near  To  we  use  the  same  technique  except  that  we  note  that  in  (14)  the 
S  functions  have  already  been  substituted  out,  and  instead  of  using  pseudo- 
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reinitialization  after  each  step,  we  use  true  reinitialization.  To  discretize  kN  we 
use  the  formulation  (15)  with  central  finite  differences  used  to  approximate  all 
derivatives. 

3.2  Fast  Sweeping  Reinitialization 

Because  of  the  frequency  with  which  we  solve  (7)  and  (9)  to  steady  state,  it  is 
imperative  that  we  have  a  fast  solution  method.  Fortunately,  there  has  been 
progress  made  in  this  area  recently.  Solution  methods  known  as  fast  sweeping 
[36], [27],  [32],  and  fast  marching  [29]  take  advantage  of  the  hyperbolic  nature  of 
the  problems  to  sweep  or  march  along  characteristics  to  derive  the  steady  state 
solution  with  a  minimal  number  of  operations  per  gridpoint.  For  reinitialization 
on  manifolds,  however,  the  straightforward  extension  of  these  fast  methods  to 
the  projected  eikonal  equation 

l/V.VOI  =  (18) 

does  not  work,  as  (18)  does  not  satisfy  the  requirements  for  the  currently  de¬ 
signed  sweeping  and  marching  methods.  This  was  noted  in  [21]  where  a  fast 
method  was  proposed  for  solving  (18)  by  instead  solving  (6)  in  a  small  band 
around  the  set  {ip  =  0}.  As  the  width  of  this  band  goes  to  0,  the  solution 
converges  to  that  of  (18). 

The  method  we  use  is  to  use  the  banded  domain  method  of  [21]  to  define  the 
locations  where  will  be  evolved,  and  then  employ  the  fast  sweeping  technique 
to  solve  (6).  Some  modifications  are  done  to  the  methods  to  fit  in  our  framework. 
Firstly,  to  avoid  the  overhead  of  initializing  the  band  (and  to  keep  the  framework 
of  the  fast  sweeping  algorithm  fixed),  we  instead  solve  the  problem 

R(x)(Ch(ip(x))  +e)’ 

where  again  Ch  is  a  piecewise  constant  cutoff  function,  taking  only  values  {0, 1}, 
with  support  width  h,  and  0  <  e  dx.  Also,  to  obtain  a  signed  distance 
function  as  our  solution  we  store  the  sign  of  at  each  point  prior  to  starting 
the  evolution,  and  then  let  (/)  =  \<p>\  before  starting  the  sweeping  procedure  (note 
that  the  sweeping  procedure  then  requires  an  initialization  of  every  point  away 
from  a  fixed  band  where  \<f>\  <  S  =  1.5 dx  to  a  large  positive  value).  After  the 
sweeping  iterations  have  ended  we  correct  each  (p  by  multiplying  it  by  its  original 
sign. 

We  note  that  in  practice,  because  of  the  frequent  perpendicularizations  and 
reinitializations,  the  local  coordinate  system  of  </>,  ip  inside  of  each  cross  section 
of  Tr  resembles  that  of  two  perpendicular  axes,  thus  the  projected  Hamiltonian 

\Pv^4>\  =  VIV0I2  -  |V0  •  Wl2  «  |V4  (20) 

as  V0  •  ~  0.  Thus  reasonable  solutions  can  be  obtained  with  large  band 

widths,  h. 
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The  exact  formulas  used  in  the  Gauss- Seidel  sweeping  step  for  2d  are  listed 
in  [36],  and  a  procedure  for  deriving  them  is  higher  dimensions  is  given  in  [35]. 
For  completeness  we  list  them  for  3d  here. 

Given  a  uniform  grid  with  spacing  dx ,  with  function  values  indexed  by  Uijj c, 
we  discretize  (19)  (with  the  right  hand  side  forcing  term  replaced  by 


[«**  -  <min)  +  }2  +  [(<*,*  -  <min)  +  ]2  +  [«,  fc  -  <mi  J  +  ]2  = 


=  fi,j,kdx2i 


(21) 


where 


^ xmin 

^ ymin  min(tq^_ •>  +  )  5 
^ zmin  ULin(l^  j  k—l  •>  :j ,/c+l )  5 

and  (x)+  =  max(x,0).  At  911  we  use  one  sided  differences  consisting  only  of 
points  lying  within  H. 

Assuming  we  have  ordered  the  oq  from  smallest  to  largest  by  a\  <  a2  <  <23, 
then  the  solution  to 


[( U  -  ai)+}2  +  [( u  -  a2)+]2  +  [(•'■  -  a3)+}2  =  f2dx2, 


which  is  the  same  as  (21),  is  given  by  the  following  formulas: 


u  = 


Hi  =  cli  T  fdx 

..  —  qi+Q2  +  [2(/^)2-(q2-qi)2]1/2 

/i2  —  2 

_  (Z)Li  ad  +  [3(/^)2-2(Q:Li  Q2)-QiQ2-QiQ3-Q2Q3)]1/2 
P3  —  3 


if  Hi  <  a2, 
else  if  H2  <  ^3, 
otherwise. 


(22) 


3.3  Pseudo-reinitialization 

As  noted  in  [4]  when  reinitializing  after  an  advancement  near  the  tube  boundary, 
we  cannot  use  straightforward  eikonal  equation  solvers  as  we  lose  interesting 
portions  of  solutions  that  differ  from  the  viscosity  solution.  To  remedy  this 
problem  we  use  the  same  type  of  pseudo-reinitialization  as  was  introduced  in 

[4]- 

The  idea  is  that  instead  of  using  the  PDE  method  of  reinitialization  to  a 
weighted  signed  distance  function  [30]  which  solves  the  equation 

0t+5(0)(|V0|--shy)=O,  (23) 

we  instead  solve 

&  +  S(0)(v^-|^-  m)=0,  (24) 
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where  y  is  a  static  vector  field  found  by  taking  y  =  V</>  prior  to  starting  the 
pseudo-reinitialization. 

To  solve  (24)  we  first  choose  y.  This  is  done  in  a  Godunov  type  upwind 
manner.  For  each  gridpoint  Xij  we  make  the  following  choices: 

rji  =  maxmod(ma,x(D~ (f)ij,k ,  0) ,  min (T>+ c ,  0 ))/dx, 
t]2  =  mamo4max(T“0y)  fe,  0),  min(T+ 0))/^, 
r]s  =  mamo(i(max(JD“0y)fe,  0),  min  (74+0^^,  0))/dz,  (25) 

if  S{(j)i^k)  >  0,  and 

771  =  maxmod(min(T)“0ijjjfe,O),max(T)^"0ijjjfe,O))/da;, 

772  =  mamod(min(i)y  ^}J- fc,  0),  max(D+ fc,  0))/^, 

773  =  maxmod(min(D“^?j;/c,  0),  max  (14 +0^-^,  0))/d2,  (26) 

if  S((f)ij^k)  <  0,  where 

7/  \  f  x  if  \x\  >  \y\, 

maxmodix.y)  =  <  4  . 

v  '  y  y  otherwise. 

Here 


Dt4>iJtk  =  ±(4>i±l,j,k  ~  0i,j,fe), 

Dyfajik  =  ±(0i,j±l,fc  “  0i,j,fe)j 
-^2  4^1, j,k  ~  =d=  (d^i, j , 1 

We  note  that  more  accurate  W/ENO  methods  can  also  be  used  to  construct 
y.  At  <911  we  enforce  that  there  will  be  no  incoming  characteristics  by  taking 
Vi  =  0  if  Vi  has  the  sign  of  an  incoming  characteristic,  e.g.  at  the  left  boundary 
in  x  we  take  Vfi  =  min(Vfi,0).  These  boundary  conditions  are  consistent  with 
those  used  in  (21),  which  employ  an  approximation  to  the  Neumann  boundary 
conditions,  d(j)/dn  =  0,  when  incoming  characteristics  are  found. 

Once  y  has  been  chosen  we  solve  (24)  using  a  fast  sweeping  method  adapted 
to  this  linear  PDE.  The  basic  form  of  the  steady  state  PDE  is 


V  •  V</>  =  /,  (27) 

where  in  this  case  V  =  S((/))y/\y\,  f  =  S(^))\rj\.  In  this  case  the  Godunov 
Hamiltonian  is  found  by  upwinding  depending  on  the  sign  of  Vi .  So  we  discretize 
4>x,  for  example,  by 


,  ^  /  (< Pi,j,k  -  4>i-i,j,k)/dx,  if  Vi  >  0, 
9x  ~  l  (4>i+ij,k  ~  <Pij,k)/dx,  if  V  <  0. 

Thus  we  can  write  the  discretized  version  of  (27)  as 
3 

^  ^  V; (Uf 0i,y. /,■  "t“  bi<f> offset^ )/ dXi  =  /, 


(28) 


(29) 
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where  cq,  bi  E  {  —  1,1},  and  0offseq  is  found  from  equations  analogous  to  (28) 
by  taking  the  indices  of  the  point  chosen  in  the  approximation  of  (j)Xi  that  is 
different  from  0^-^.  If  we  note  the  dependence  of  oq,  bi  on  the  sign  of  Vi,  then 
we  can  write  (29)  as 


3 

^  ^  |  Id  |  (0i,j,&;  0offset  i)/dXi  =  f. 
i=  1 

Then  solving  for  4>ij:k  we  find 


/  +  Ej=l  I^K^offsetJ/tfaj 

ELi  \Vi\/dxi 


(30) 


(31) 


If  Vi  =  0  then  is  does  not  matter  which  offset  point  we  choose,  and  in  the  cases 
where  X^=i  |Id|  ~  0  we  set  0^-^  to  the  average  of  its  neighbors  with  indices 
(a,  6,  c)  such  that  ||  (a,  6,  c)  —  (i,  j,  k)  ||q  <  1. 

The  sweeping  directions  used  are  the  same  as  those  used  for  the  eikonal 
equation,  however,  we  do  not  initialize  the  grid  to  large  positive  values  away 
from  the  fixed  band.  Rather  outside  the  fixed  band  where  |0|  <  S  =  1.5 dx  we 
use  the  current  values  of  0  as  initial  values. 


3.4  Perpendicularization 

For  perpendicularization  we  note  that  (9)  can  be  discretized  as  (27)  with  /  =  0 
and  V  =  S'(0)V0/|V0|.  The  solution  to  this  PDE  is  found  by  fast  sweeping 
using  the  same  framework  and  initialization,  including  Gauss-Seidel  sweeps  us¬ 
ing  (31)  as  was  done  in  the  pseudo-reinitialization  case.  In  this  case  the  fixed 
band  is  taken  where  |0|  <  5  =  1.5 dx.  The  vector  V  is  found  using  central  dif¬ 
ferencing,  where  we  regularize  the  denominator  in  V ,  and  correct  for  incoming 
characteristics  as  well. 


3.5  Topology  Preservation 

For  certain  problems  such  as  searching  it  makes  physical  sense  that  the  search 
path  To  enters  from  one  point  a  E  dd,  and  leaves  through  another  point 
b  E  dQ  and  has  fixed  topology.  For  codimension- 1  level  set  dynamics  there  is  a 
method,  outlined  in  [13],  that  guarantees  topology  preservation.  This  was  used 
in  [4]  to  keep  the  path  from  changing  topology  in  2d.  However,  for  codimension- 
2  curves  in  3d  there  is  no  existing  algorithm  of  this  type.  Therefore,  in  the  spirit 
of  the  projected  PDEs  with  which  we  have  been  working,  we  attempt  to  project 
the  2d  method  from  [13]  onto  the  surface  with  normal  V^/IV^I- 

The  idea  is  that  when  we  are  changing  the  value  of  if  the  sign  is 

being  changed  and  we  are  near  the  set  {'0  =  0},  then  we  attempt  to  find  a  local 
neighborhood,  N,  of  0  values  on  {0  =  0},  and  project  N  to  the  standard  2d  nine 
point  neighborhood  where  the  topology  preservation  can  be  enforced.  To  find 
N  we  first  calculate  the  normal  vector  to  0:  n ^  =  V0(x^j?/c)/|V0(x^j?/c)|  using 
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Figure  3:  Projection  of  an  8  point  3d  neighborhood  onto  a  2d  plane.  The  bold 
lines  represent  the  boundary  of  the  neighborhood. 


central  finite  differences.  Then  we  find  the  normal  vector  w  that  approximates 
n ^  such  that  w  G  W  =  {xa,b,c  ~  xi,j,k},  where  ||(a,6,c)  -  (i,j,k) =  1.  We 
do  this  by  taking  the  w  eW  that  maximizes  n^-w/\w\.  Once  this  approximate 
normal  is  found  we  fill  N  with  all  points  such  that  ( xa^,c  ~  xi,j,k)  •  w  =  0,  where 
II (a,  6,  c)  —  (i,j,  /c)||Zoo  =  1,  i.e.  we  take  points  from  the  26  nearest  neighbors  to 
Xij:k  that  he  on  the  plane  passing  through  Xijj c  with  normal  w.  The  cardinality 
of  TV  is  8  unless  \\w\lh  =  dx  +  dy  +  dz,  in  which  case  it  is  6. 

Next,  we  project  N  onto  a  local  2d  grid  by  finding  a  nonzero  component 
of  w  and  projecting  to  the  plane  perpendicular  to  this  component  direction. 
For  example,  if  w  =  (1,0, 1),  then  we  could  project  any  node  xa^,c  €=  TV  onto 
yb,c  or  Da^-  Once  we  have  this  2d  neighborhood,  all  of  whose  points  lie  on  a 
regular  2d  grid,  we  can  apply  the  standard  topology  preservation  method  from 
[13].  See  Figure  3  for  an  example  of  how  N  is  projected  onto  a  2d  plane  when 
card(N)  =  8. 

If  card(N)  =  6  then  we  need  to  fill  out  the  projected  2d  neighborhood  with  2 
extra  points,  or  else  our  algorithm  will  be  too  severe  in  its  judgment  of  whether 
or  not  topology  has  changed.  Thus  we  examine  all  three  symmetric  extensions 
of  N  that  include  the  points,  &,c,  that  are  closest  to  Xijj c,  are  lying  on  our 
grid,  and  have  the  property  that  (xa^,c  ~  xi,j,k)  •  w  =  0.  These  extra  points 
have  the  property  that  exactly  one  of  the  indices  {a,  6,  c}  is  offset  from  the  index 
{i,  j,  k}  by  ±2.  Figure  4  shows  an  example  of  the  hexagonal  neighborhood  in  3d, 
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Figure  4:  Projection  of  a  6  point  3d  neighborhood  onto  a  2d  plane.  The  bold 
lines  represent  the  boundary  of  the  neighborhood.  The  hexagon  along  with 
either  {^4i,  ^2},  {#1,  ^2},  or  {Ci,  C2}  makes  up  an  8  point  neighborhood  that 
can  be  mapped  to  a  standard  8  point  rectangular  neighborhood  of  the  center 
point. 


along  with  its  three  possible  extended  symmetric  8  point  projections  onto  a  2d 
plane,  each  of  which  can  be  mapped  in  a  one  to  one  manner  onto  a  rectangular 
8  point  neighborhood  where  the  usual  topology  preservation  algorithm  can  be 
run.  For  points  where  card(N)  =  6  we  run  the  topology  change  test  on  all  three 
possible  extended  neighborhoods,  and  prohibit  a  sign  change  in  <p  at  if  any 
of  the  three  tests  indicates  a  topology  change. 

While  the  work  done  in  constructing  N  is  more  significant  than  what  was 
done  in  the  2d  case,  the  codimension-2  nature  of  To  means  that  this  proce¬ 
dure  must  be  applied  an  order  of  magnitude  fewer  times  during  the  evolution. 
In  practice  we  require  |^|  <  2 dx  before  the  topology  preservation  method  is 
applied. 

We  also  note  that  if  it  is  necessary  to  keep  the  points  where  To  intersects 
the  boundary  (or  any  other  subset  of  Cl)  fixed,  we  imposed  Dirichlet  boundary 
conditions  (f)  =  i/j  =  0  at  these  points.  If  these  points  do  not  lie  on  the  uniform 
grid  then  we  can  modify  the  grid  slightly  near  them  so  that  they  are  included 
in  the  discretization  of  Cl.  If  this  is  done  then  a  local  method  for  advancing  the 
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solution  on  an  unstructured  grid  could  be  used  near  the  points. 

3.6  Outline  of  Evolution  Procedure 

In  this  section  we  outline  the  evolution  procedure.  We  give  a  listing  of  the  steps 
taken  during  one  iteration.  The  evolution  procedure  is  repeated  until  steady 
state  is  reached. 

In  certain  steps  of  the  computational  procedure  we  shift  </>,  ip  by  adding  or 
subtracting  {a^bi)  at  all  points  so  that  identical  equations  being  solved  near 
r(a.jb.)  can  be  solved  using  the  same  coded  functions  for  all  i.  This  is  explained 
in  this  way  to  emphasize  that  coding  can  be  done  using  a  smaller  number  of 
functions  that  do  identical  jobs  on  shifted  versions  of  the  data.  When  this  is 
done  we  denote  the  shifted  version  of  <p  as  cp  +  a*.  It  is  assumed  that  after  the 
step  in  question  is  completed,  that  (p  is  then  shifted  back  the  opposite  way  by 
—di.  This  is  done  similarly  with  ip. 

The  evolution  loop  advancing  the  solution  from  time  t\  to  t\  +  dt  is  given 
below.  We  illustrate  the  steps  with  an  example  PDE  system  of  the  form: 

4>t  =  P{x)5{r  -  ^TV)^==  ~  XkN  ■  V<p, 

V  4>2  +  ip2 

ipt  =  P{x)5(r  -  y Pp2  +  V>2)  =  -  XkN  • 

vV2  +  V>2 

where  we  will  use  the  substitutions 

5(r  -  \[W+  V>2)  — >  |V(/>|Cr(r  -  ||(0,V’)ll)>  in  (32a)  or 

S(r  -  vV2  +  V’2)  |VV>| Cr(r  -  \\(<p,  V7)  II)  in  (32b), 

when  they  are  implemented  numerically. 

1.  Advance  functions  near  T(a.?b.),  for  i  =  1, . .  .M  where  we  choose  (aq,^) 
such  that  ||(a*,^)||  =  r. 

(a)  Find  r]  «  V0  based  on  (p  —  ip  —  bi  using  (25),  (26). 

(b)  Evolve  <p  from  time  t\  to  t\  +dt  the  points  near  where  <p  =  a^ip  — 
i.e.  evolve  all  (pt  PDE  terms  with  5(r  —  \j  (p2  +  ip2)  in  them,  e.g. 

pt  =  P(x)5(r  -  vV>2+^2)  ,  =  ■ 

v  4>  +  w 

(c)  Pseudo-reinitialize  <p  using  a  banded  fast  sweeping  method  within  a 
fixed  band  around  {ip  —  bi  =  0},  using  {</>  —  cq  «  0}  as  the  points 
where  (p  is  fixed  during  the  sweeping  process. 

(d)  Find  r]  ~  Vip  based  on  <p  —  di,ip  —  bi  using  equations  (25),  (26)  with 
ip  substituted  in  place  of  (p . 


(32a) 

(32b) 

(33) 
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(e)  Evolve  0  from  time  t\  to  t\  +  dt  the  points  near  where  0  =  0  =  bi, 

i.e.  evolve  all  'ipt  PDE  terms  with  5(r  —  \J  (p2  +  02)  h1  them,  e.g. 


ijjt  =  P(x)S(r  -  a/02+^2)  /  . 2  /2- 

V<P  +  ip2 

(f)  Pseudo-reinitialize  0  using  a  banded  fast  sweeping  method  within  a 
fixed  band  around  with  {0  —  =  0},  using  {0  —  bi  ~  0}  as  the  points 

where  0  is  fixed  during  the  sweeping  process. 

2.  Advance  functions  near  Tq. 

(a)  Evolve  0  from  time  t\  to  t\  +  dt  the  points  near  where  0  =  0  =  0, 
i.e.  evolve  all  <pt  PDE  terms  with  8{y/ (p2  +  02)  in  them,  e.g. 

0t  =  -XkN  •  V0. 

(b)  Reinitialize  0  using  a  banded  fast  sweeping  method  within  a  fixed 
band  around  with  {0  =  0},  using  {0  ~  0}  as  the  points  where  0  is 
fixed  during  the  sweeping  process. 

(c)  Perpendicularize  0  with  fast  sweeping,  using  {0  ~  0}  as  the  points 
where  0  is  fixed  during  the  sweeping  process. 

(d)  Evolve  from  time  t\  to  t\  +  dt  the  points  near  where  (p  =  0  =  0, 
i.e.  evolve  all  0t  PDE  terms  with  5(^/<p2  +  ip2)  in  them,  e.g. 

0t  =  —XkN  •  V0. 

(e)  Reinitialize  0  using  a  banded  fast  sweeping  method  within  a  fixed 
band  around  with  {0  =  0},  using  {0  «  0}  as  the  points  where  0  is 
fixed  during  the  sweeping  process. 

(f)  Perpendicularize  0  with  fast  sweeping,  using  {0  «  0}  as  the  points 
where  0  is  fixed  during  the  sweeping  process. 


4  Numerical  Simulations 


In  this  section  we  present  some  numerical  simulations.  The  PDE  we  evolve  to 
steady  state  is  (32),  using  the  methods  mentioned  above.  The  domain  D  is 
[ —  1, 1] 3  for  all  problems,  discretized  in  a  uniform  rectangular  grid.  A  conserva¬ 
tive  estimate  on  the  CFL  condition  for  the  problem  is 


dt  max 


\p\ 

dx 


\p_ i 

dy 


max 


1 

dy 


<  1, 


(34) 


where  &max  is  the  magnitude  of  the  maximum  curvature  that  we  allow  to  be 
discretized  on  the  grid.  In  practice  this  is  set  at  1/ dx.  We  use  the  max  applied  to 
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the  P  and  k  terms  individually  instead  of  to  their  sum  because  we  are  splitting 
the  evolution  procedure. 

For  the  individual  examples  we  do  not  explicitly  write  the  initial  conditions, 
but  rather  show  them  in  contour  plots.  The  way  they  are  constructed  is  by 
determining  an  initial  curve  To(t  =  0)  and  finding  arbitrary  functions  that  have 
{( j)(t  =  0)  =  0}  fl  {'0( t  =  0)  =  0}  =  To(t  =  0),  and  then  running  reinitializations 
and  perpendicularizations  if  necessary.  More  details  about  the  initializations 
can  be  found  in  [2],  or  in  [34]  where  Clebsch  variables  (from  hydrodynamics) 
that  are  constant  along  particle  paths  are  discussed. 

For  some  figures  the  energy  vs.  time  steps  plots  are  shown,  where  the  energy 
to  be  maximized  is  defined  by  (4)  summed  with  —  A|T|  using  (10).  The  5  and 
Heaviside  functions  used  are  the  compactly  supported  ones  given  in  [5],  with 
support  parameter  e  =  2 dx.  It  should  be  noted  that  a  more  accurate  numerical 
construction  of  these  singular  functions  can  be  found  in  [11]  should  a  more  exact 
measure  of  the  energy  be  needed. 

In  Figure  5  we  show  an  example  where  Sr  is  a  tube  of  radius  0.2  and  there  is 
an  obstacle  (a  sphere  with  a  tunnel  through  it)  where  P(x )  =  —  1.  Outside  of  the 
obstacle  P(x)  =  0.  Here  we  fix  the  boundary  of  the  tube  where  x  =  {±1}.  The 
regularization  parameter  A  =  0.01.  We  use  a  uniform  rectangular  discretization 
with  dx  =  dy  =  dz  =  2/50.  The  number  of  Sr  advancements  is  M  =  5.  At 
each  time  step  these  are  chosen  using  (17),  where  0q  is  chosen  randomly.  In 
the  figure  it  is  seen  that  the  tube  locates  the  tunnel  and  avoids  the  areas  where 
P(x)  <  0. 

In  Figure  6  we  show  two  examples  starting  form  identical  initial  conditions, 
but  where  topology  preservation  is  enforced  in  one  example  but  not  in  the  other. 
Here  Sr  is  a  tube  of  radius  0.2,  and  there  are  two  balls  where  P{x)  >  0,  a  box 
where  P(x)  <  0,  and  P{x)  =  0  elsewhere.  The  regularization  parameter  A  = 
0.02.  We  use  a  uniform  rectangular  discretization  with  dx  =  dy  =  dz  =  2/50. 
The  number  of  Sr  advancements  is  M  =  4.  At  each  time  step  these  are  chosen 
using  (17),  where  6q  is  chosen  randomly.  In  the  figure  we  see  that  the  topology 
is  prohibited  from  changing  when  the  topology  preservation  is  enforced. 

In  Figure  7  we  show  an  example  where  pseudo-reinitialization  allows  for 
cusped  regions  to  form.  Here  Sr  is  a  tube  of  radius  0.2,  and  there  is  a  rectangle 
where  P(x)  >  0.  The  regularization  parameter  A  =  0.01.  We  use  a  uniform 
rectangular  discretization  with  dx  =  dy  =  dz  =  2/50.  The  number  of  Sr 
advancements  is  M  =  4.  At  each  time  step  these  are  chosen  using  (17),  where  0q 
is  chosen  randomly.  The  figure  demonstrates  how  cusped  regions  are  allowed  to 
form  when  pseudo-reinitialization  is  used  as  opposed  to  standard  reinitialization. 
If  standard  reinitialization  is  used  instead  of  pseudo-reinitialization,  then  we  do 
not  capture  the  cusped  region  and  the  result  is  that  of  Figure  8. 

In  Figure  9  we  show  an  example  where  the  path  width  is  spatially  varying. 
In  this  example  R(x)  =  1  when  x  <  0,  while  R(x)  =  2  when  x  >  0.  Here  Sr  is 
a  tube  of  radius  0.15,  and  there  is  a  sphere  where  P(x)  =  —1  centered  near  the 
origin.  The  regularization  parameter  A  =  0.02.  We  use  a  uniform  rectangular 
discretization  with  dx  =  dy  =  dz  =  2/50.  The  number  of  Sr  advancements 
is  M  =  5.  At  each  time  step  these  are  chosen  using  (17),  where  0q  is  chosen 
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Figure  5:  Top,  left  to  right:  Initial  condition,  boundary  of  region  where  P  <  0, 
final  solution.  Bottom,  left  to  right:  Final  solution  together  with  boundary  of 
region  where  P  <  0,  and  energy  vs.  time  steps  plot. 
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Figure  6:  Left  to  right:  Boundary  of  sets  where  P  >  0  (blue  spheres)  and  P  <  0 
(black  box),  initial  condition  of  tube,  final  tube  with  no  topology  preservation, 
final  tube  with  topology  preservation. 
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Figure  7:  Left  to  right:  Initial  T  and  boundary  of  rectangle  where  P  >  0, 
advanced  T,  and  advanced  Sr •  In  this  example  pseudo-reinitialization  is  used. 


Figure  8:  Left  to  right:  Initial  Sr  and  boundary  of  rectangle  where  P  >  0, 
and  advanced  Sr-  In  this  example  standard  reinitialization  is  used  instead  of 
pseudo-reinitialization. 
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Figure  9:  Left  to  right:  Boundary  of  set  where  P  <  0,  initial  Sr,  and  final  Sr- 
In  this  example  R{x)  is  spatially  varying. 


randomly. 

5  Conclusion 

We  have  presented  an  extension  of  the  level  set  based  algorithm  for  solving 
a  variational  approach  to  path  planning  that  was  originally  proposed  in  [4]. 
This  involved  adapting  the  codimension-2  level  set  framework  established  in 
[2]  for  motions  of  curves  in  M3.  Some  key  features  of  this  algorithm  are  the 
energy  integrals  used  to  define  the  search  criteria,  the  splitting  technique  used  to 
advance  the  PDEs,  the  fast  methods  for  reinitialization,  pseudo-reinitialization, 
prependicularization,  and  the  topology  preservation  for  curves  in  M3. 

The  energy  integrals  used  are  very  basic  and  encompass  general  properties 
that  are  desirable  in  many  path  planning  problems.  However,  they  are  not 
exhaustive  and  more  complicated  energies  based  on  functionals  of  curvature, 
torsion,  or  other  path  properties  can  be  constructed. 

Some  other  problems  which  we  have  not  approached  but  are  feasible  for 
future  research  are:  multiple  non-intersecting  paths,  time  dependent  parameters 
such  as  R,  P,  /i,  paths  passing  through  multiple  prescribed  points,  and  self- 
intersecting  paths.  Also,  it  may  be  possible  to  use  the  ideas  from  [23]  to  use  this 
3d  method  to  construct  self-intersecting  paths  for  problems  with  2d  domains. 
Level  set  motion  of  tubes  having  small  positive  width  may  also  have  applications 
in  image  processing,  such  as  segmentation  of  filaments  or  other  thin  objects  such 
as  blood  vessels. 
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